#ifndef _DDMCMC_MATRIX_H
#define _DDMCMC_MATRIX_H

#include <iostream>
#include <fstream>
#include <memory.h>
#include <stdlib.h>
#include <math.h>
#include <new.h>
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <opencv2\core\core.hpp>
#include <opencv2\imgproc\imgproc.hpp>

using namespace std;

#define LPI	3.1415926535897931

typedef double* pdouble;

template<class Tp>
class SimpleMatrix
{
public:
	// constructions & destructions
	SimpleMatrix();
	SimpleMatrix(int row, int col);
	~SimpleMatrix();

	// operators
	__forceinline Tp&		operator()(const int row, const int col);
	__forceinline Tp&		operator()(const long index);
	SimpleMatrix<Tp>&		operator=(const SimpleMatrix<Tp>& x);

	// operations
	int				SetDimension(const int row, const int col);
	void			GetDimension(int& row, int& col) const;
	__forceinline long		rows() const{ return nrow; };
	__forceinline long		cols() const{ return ncol; };
	__forceinline long		size(void) const { return rows()*cols(); };
	__forceinline bool		Valid(const int row, const int col) const;
	__forceinline bool		Valid(const int row) const;
	__forceinline Tp&		Data(const int row, const int col) const;
	__forceinline Tp&		Data(const int index) const;
	__forceinline	void	InitValue(Tp value);

protected:
	virtual int		MatrixAlloc(const long lSize);
	virtual int		MatrixFree();
	virtual int		MatrixAlloc(const int row, const int col);

protected:
	Tp		*data;
	Tp		**pdata;
	int		nrow, ncol;

};


template<class Tp>
SimpleMatrix<Tp>::SimpleMatrix()
{  //null matrix
	nrow = 0; ncol = 0;
	data = NULL;
	pdata = NULL;
}

template<class Tp>
SimpleMatrix<Tp>::SimpleMatrix(int row, int col)
{
	SimpleMatrix();

	//initialize matrix
	if (row > 0 && col >0)
	{
		nrow = row;
		ncol = col;
		if (MatrixAlloc(nrow, ncol))
		{
			cerr << "\n Memory allocation Error ! ";
			exit(-1);
		}
	}
}
template<class Tp>
SimpleMatrix<Tp>::~SimpleMatrix()
{
	MatrixFree();
}

template<class Tp>
int SimpleMatrix<Tp>::MatrixFree()
{  //null matrix

	if (data != NULL)
		delete[] data;           // STEP 2: DELETE THE ROWS
	data = NULL;
	if (pdata != NULL)
		delete[] pdata;
	pdata = NULL;

	nrow = ncol = 0;

	return 0;
}

template<class Tp>
int SimpleMatrix<Tp>::MatrixAlloc(const long lSize)
{

#ifdef _DEBUG
	assert(lSize >= 0);
#endif

	try
	{ 								//test for exceptions
		data = new Tp[lSize];	//allocate memory for rows
		pdata = new Tp*[nrow];
		long i;
		for (i = 0; i<nrow; i++)
			pdata[i] = &(data[ncol*i]);
	}
	catch (double*)
	{  // ENTER THIS BLOCK ONLY IF xalloc IS THROWN.
		return -1;
	}
	return 0;
}

template<class Tp>
int SimpleMatrix<Tp>::MatrixAlloc(const int row, const int col)
{
	if (row >= 0 && col >= 0)
		return MatrixAlloc(((long)row) * col);
	else
		return -1;
}

template<class Tp>
__forceinline Tp& SimpleMatrix<Tp>::operator ()(const int row, const int col)
{
#ifdef _DEBUG
	assert(row < nrow && row >= 0);
	assert(col < ncol && col >= 0);
#endif

	//	return data[((long)row) * cols() + col];
	return pdata[row][col];
}

template<class Tp>
__forceinline Tp& SimpleMatrix<Tp>::operator ()(const long index)
{
#ifdef _DEBUG
	assert(index<((long)rows()) * cols() && index >= 0);
#endif
	return data[index];
}

template<class Tp>
SimpleMatrix<Tp>& SimpleMatrix<Tp>::operator=(const SimpleMatrix<Tp>& x)
{//performs assignment statement

	//beware of self assignments : x = x
	if (this != &x)
	{
		//register i,j;
		int i, j;

		if (rows() != x.rows() || cols() != x.cols())
		{
			//delete the existing data;
			MatrixFree();

			nrow = x.nrow; ncol = x.ncol;
			MatrixAlloc(nrow, ncol); //allocate memory
		}

		for (i = 0; i<nrow; i++)
			for (j = 0; j<ncol; j++)
				(*this)(i, j) = x.Data(i, j);
	}

	return *this;
}

template<class Tp>
__forceinline bool	SimpleMatrix<Tp>::Valid(const int row, const int col) const
{
	if (row >= 0 && row<rows() && col >= 0 && col<cols())
		return true;
	else
		return false;
}

template<class Tp>
__forceinline bool	SimpleMatrix<Tp>::Valid(const int row) const
{
	if (row >= 0 && row<size())
		return true;
	else
		return false;
}


template<class Tp>
Tp& SimpleMatrix<Tp>::Data(const int row, const int col) const
{
#ifdef _DEBUG
	assert(row < nrow && row >= 0);
	assert(col < ncol && col >= 0);
#endif

	//	return data[((long)row) * cols() + col];
	return pdata[row][col];
}

template<class Tp>
Tp& SimpleMatrix<Tp>::Data(const int index) const
{
#ifdef _DEBUG
	assert(index >= 0 && index<size());
#endif

	return data[index];
}

template<class Tp>
int SimpleMatrix<Tp>::SetDimension(const int row, const int col)
{  //set the dimension and allocates the memory of the matrix

	if (row != nrow || col != ncol)
	{
		//delete the existing data;
		MatrixFree();

		nrow = row;
		ncol = col;

#ifdef _DEBUG
		assert(nrow >= 0);
		assert(ncol >= 0);
#endif
		if (row>0 && col>0)
			MatrixAlloc(row, col);
	}

	return 0; //could be used to return the success
	// or failure of allocation
}

template<class Tp>
void SimpleMatrix<Tp>::GetDimension(int& row, int& col) const
{  //report the dimension of the matrix
	row = nrow;
	col = ncol;
}

template<class Tp>
__forceinline void SimpleMatrix<Tp>::InitValue(Tp value)
{
	//register i,j;
	int i, j;

	if (data != NULL)
	{
		for (i = 0; i < rows(); i++)
			for (j = 0; j < cols(); j++)
				(*this)(i, j) = value;
	}
}

/* =========================================================================
   ================== end of SimpleMatrix ==================================
   =========================================================================
*/

template<class Tp>
class Vector;

template<class Tp>
class Raster;


template<class Tp>
class Matrix
{
	

public:
	//CONSTRUCTION
	Matrix(); // Tested
	Matrix(int row, int col); // Tested
	Matrix(const Matrix& x); // Tested
	~Matrix();

	//OPERATORS
	inline Matrix<Tp> &		operator=(const Matrix<Tp>& x); // Tested
	__forceinline Tp&		operator()(const int row, const int col); // Tested
	__forceinline Tp&		operator()(const long index); // Tested

	Matrix operator+(const Matrix &x); // Tested
	Matrix operator-(const Matrix &x); // Tested
	Matrix operator*(const Matrix &x); // Tested
	Vector<Tp> operator*(const Vector<Tp> &x);


	friend ostream &operator<< (ostream &strm, Matrix& mat){ return strm << mat.data << endl; }; // Tested
	void operator>>(ofstream &strm);
	void operator<<(ifstream &strm);

	void			Set(Tp t); // Tested

	//virtual bool	Save(char *fname);

	Matrix			operator+(const Tp y); // Tested
	Matrix			operator-(const Tp y); // Tested
	Matrix			operator*(const Tp y); // Tested
	Matrix			operator/(const Tp y); // Tested
	Matrix&			operator^(const Matrix y); // Tested

	int				SetDimension(const int row, const int col); // Tested

	bool			ExpandTo(const int rows_new, const int cols_new); // Tested
	void			GetDimension(int& row, int& col) const; // Tested
	__forceinline long rows() const { return nrow; }; // Tested
	__forceinline long cols() const { return ncol; }; // Tested
	__forceinline long size() const { return nrow*ncol; }; // Tested
	int				nx() const{ return ncol; }; // Tested
	int				ny() const{ return nrow; }; // Tested

	__forceinline bool		Valid(const int row, const int col) const; // Tested
	__forceinline bool		Valid(const int row) const; // Tested
	Matrix			T() const; // Tested
	virtual void	Identity(void);					// set this matrix to be I matrix // Tested
	double			Det() const; // Tested
	double			DetNoZero() const; // Tested
	Matrix			Inv() const;  // Tested
	virtual void	Zero(void);						// set this matrix to be zero matrix // Tested
	__forceinline Tp		Data(const int row, const int col) const; // Tested
	__forceinline Tp		Data(const int index) const; // Tested
	__forceinline	void	InitValue(Tp value); // Tested
	virtual Tp		GetMin(void) const; // Tested
	virtual Tp		GetMax(void) const; // Tested
	virtual void	GetMinMax(Tp &value_min, Tp &value_max); // Tested

	virtual Tp		Sum(void) const; // Tested
	virtual Tp		SumL2(void) const; // Tested
	virtual Tp		Mean(void) const; // Tested
	virtual Matrix<Tp>	mabs(void) const; // Tested
	virtual void	GetPos(const long index, long &j, long &i); // Tested
	virtual Tp		Dist(const Matrix<Tp> &a);

	// convolution
	virtual Matrix<Tp>	ConvHor(const Matrix& kern);	// horizontal convolution // Tested
	virtual Matrix<Tp>	ConvVer(const Matrix& kern);	// vertical convolution  // Tested
	virtual void	NormToZero(void);				// make sum up to zero // Tested

	// kernel generation
	virtual void	SetGaussian(int dim = 1, double sigma = 2, int direvitive = 0);	// Gaussian kernel
	virtual void	SetGabor(const double k, const double sigma, const double theta = 0 /* in degree */);  // Tested


public:
	virtual int		MatrixFree(); // tested
	void			Eigen(Matrix& eigenvalue, Matrix& eigenvector); // tested

public:
	cv::Mat		data;
	int			nrow, ncol;

private:

	virtual int MatrixAlloc(const int row, const int col);
};

__forceinline int  IsIntegerValue(bool &bintg, const double value);
__forceinline void	FFT_1D(Matrix<complex<double> > & H, const int sign = 1, const int t = 0);
__forceinline void	FFT_shift(Matrix<complex<double> > & H, const int t = 0);
__forceinline void	IFFT_shift(Matrix<complex<double> > & H, const int t = 0);

template<class Tp1, class Tp2> void CopyMatrix(Matrix<Tp1>& mx_dest, const Matrix<Tp2>& mx_src);

template<class Tp1, class Tp2> void CopyMatrix2Raster(Raster<Tp1> &ras_dest, Matrix<Tp2> &mx_src);

template<class Tp1, class Tp2> void CopyRaster2Matrix(Matrix<Tp1> &mx_dest, Raster<Tp2> &ras_src);

template<class Tp1, class Tp2> void assign_matrix(Matrix<Tp1>& x1, const Matrix<Tp2>& x2);

template<class Tp1, class Tp2>
void assign_matrix(Matrix<Tp1>& x1, const Matrix<Tp2>& x2) // Jiadi
{
	x1.SetDimension(x2.rows(), x2.cols());

	for (int i = 0; i < x2.rows(); i++)
		for (int j = 0; j < x2.cols(); j++)
			x1(i, j) = (Tp1)(x2.Data(i, j));
}


template<class Tp1, class Tp2>
void CopyMatrix2Raster(Raster<Tp1> &ras_dest, Matrix<Tp2> &mx_src)
{
	ras_dest.SetDimension(mx_src.rows(), mx_src.cols());

	for (int i = 0; i < mx_src.rows(); i++)
		for (int j = 0; j < mx_src.cols(); j++)
			ras_dest(i, j) = (Tp1)(mx_src.Data(i, j));

}

template<class Tp1, class Tp2>
void CopyRaster2Matrix(Matrix<Tp1> &mx_dest, Raster<Tp2> &ras_src)
{
	mx_dest.SetDimension(ras_src.rows(), ras_src.cols());

	for (int i = 0; i < ras_src.rows(); i++)
		for (int j = 0; j < ras_src.cols(); j++)
			mx_dest(i, j) = (Tp1)(ras_src.Data(i, j));
}

template<class Tp>
Matrix<Tp>::Matrix()  // Jiadi
{  //null matrix
	nrow = 0; ncol = 0;
}

template<class Tp>
Matrix<Tp>::Matrix(int row, int col) // Jiadi
{
	Matrix();

	//initialize matrix
	if (row > 0 && col >0)
	{
		nrow = row;
		ncol = col;
		if (MatrixAlloc(nrow, ncol))
		{
			cerr << "\n Memory allocation Error ! ";
			exit(-1);
		}
	}
}

template<class Tp>
Matrix<Tp>::Matrix(const Matrix& x) // Jiadi
{//copy constructor

	nrow = x.nrow; ncol = x.ncol;
	data = x.data.clone();
}


template<class Tp>
Matrix<Tp>::~Matrix() // Jiadi
{  //delete the data
	MatrixFree(); //free the memory
}

template<class Tp>
void Matrix<Tp>::Set(Tp t) // Jiadi
{
	data.setTo(cv::Scalar(t));
}

template<class Tp>
int Matrix<Tp>::MatrixFree() // Jiadi
{  //null matrix

	data.release();
	nrow = ncol = 0;

	return 0;
}


template<class Tp>
bool Matrix<Tp>::ExpandTo(const int rows_new, const int cols_new) // Jiadi
{
	if (rows_new<rows() || cols_new <cols())
		return false;

	unsigned long lsize = rows_new*cols_new;

	cv::Mat data_new(rows_new, cols_new, cv::DataType<Tp>::type);
	// copy the original data
	for (int j = 0; j < rows(); j++)
		for (int i = 0; i < cols(); i++)
			data_new.at<Tp>(j, i) = Data(j, i);

	// de-allocate the original space
	MatrixFree();
	nrow = rows_new;
	ncol = cols_new;
	data = data_new;

	return true;
}


template<class Tp>
int Matrix<Tp>::MatrixAlloc(const int row, const int col) // Jiadi
{
	if (row >= 0 && col >= 0) {
		data.create(row, col, cv::DataType<Tp>::type);
		return 0;
	} else
		return -1;
}


template<class Tp>
__forceinline bool	Matrix<Tp>::Valid(const int row, const int col) const // Jiadi
{
	if (row >= 0 && row<nrow && col >= 0 && col<ncol)
		return true;
	else
		return false;
}

template<class Tp>
__forceinline bool	Matrix<Tp>::Valid(const int row) const // Jiadi
{
	if (row >= 0 && row<size())
		return true;
	else
		return false;
}

template<class Tp>
int Matrix<Tp>::SetDimension(const int row, const int col) // Jiadi
{  //set the dimension and allocates the memory of the matrix

	if (row != nrow || col != ncol)
	{
		//delete the existing data;
		MatrixFree();

		nrow = row;
		ncol = col;

		if (row>0 && col>0)
			MatrixAlloc(row, col);
	}

	return 0; //could be used to return the success
	// or failure of allocation
}

template<class Tp>
void Matrix<Tp>::GetDimension(int& row, int& col) const // Jiadi
{  //report the dimension of the matrix
	row = nrow;
	col = ncol;
}


template<class Tp>
void Matrix<Tp>::operator>>(ofstream &strm) // Jiadi
{
	// dimensions
	strm.write((char*)&nrow, sizeof(nrow));
	strm.write((char*)&ncol, sizeof(ncol));

	// data
	strm.write((char*)data.data, sizeof(Tp)*size());
}

template<class Tp>
void Matrix<Tp>::operator<<(ifstream &strm) // Jiadi
{
	int row_read, col_read;

	MatrixFree();

	// dimensions
	strm.read((char*)&row_read, sizeof(nrow));
	strm.read((char*)&col_read, sizeof(ncol));

	if (row_read>0 && col_read>0)
		SetDimension(row_read, col_read);
	// data
	strm.read((char*)data.data, sizeof(Tp)*size());
}

/*
template<class Tp>
bool Matrix<Tp>::Save(char *fname) // Jiadi
{
	ofstream	stream;

	stream.open(fname, ios::out);
	(*this) >> stream;
	if (stream.is_open())
	{
		stream.close();
		return true;
	}
	else
		return false;
}
*/

template<class Tp>
Matrix<Tp> Matrix<Tp>::operator+ (const Matrix<Tp>& x) // Jiadi
{  //add two matrices

	Matrix<Tp> temp(nrow, ncol); //intialize the temporary matrix

	temp.data = x.data + data;
	return temp; //return the new matrix
}

template<class Tp>
Matrix<Tp> Matrix<Tp>::operator+ (const Tp y) // Jiadi
{
	//add a constant to a matrices
	Matrix<Tp> temp(rows(), cols()); //intialize the temporary matrix

	temp.data = data + y;
	return temp; //return the matrix
}

template<class Tp>
Matrix<Tp> Matrix<Tp>::operator - (const Matrix<Tp>& x) // Jiadi
{  //subtract two matrix
	Matrix<Tp> temp(rows(), cols()); //intialize the temporary matrix

	temp.data = data - x.data;
	return temp; //return the new matrix
}


template<class Tp>
Matrix<Tp> Matrix<Tp>::operator - (const Tp y) // Jiadi
{  //subtract a constant from a matrix
	Matrix<Tp> temp(rows(), cols()); //intialize the temporary matrix

	temp.data = data - y;
	return temp; //return the matrix
}

template<class Tp>
Matrix<Tp> Matrix<Tp>::operator * (const Matrix<Tp>& x) // Jiadi
{  //multiply two matrices
	Matrix<Tp>	temp(nrow, x.ncol); //intialize the temporary matrix
	cv::Mat a, b;
	data.convertTo(a, CV_64F);
	x.data.convertTo(b, CV_64F);
	temp.data = a*b;
	return temp; //return the result of multiplication
}

template<class Tp>
Vector<Tp> Matrix<Tp>::operator * (const Vector<Tp>& x) 
{  //multiply two matrices
	Tp			sum;
	Vector<Tp>	temp;

	temp.SetDimension(rows(), x.cols()); //intialize the temporary Vector

	for (int i = 0; i<rows(); i++)
		for (int j = 0; j<x.cols(); j++)
		{
			sum = 0;
			for (int k = 0; k<cols(); k++)
				sum = sum + Data(i, k)*x.Data(k, j); //multiply

			temp(i, j) = (Tp)sum;
		}

	return temp; //return the result of multiplication
}

template<class Tp>
Matrix<Tp> Matrix<Tp>::operator * (const Tp y) // Jiadi
{  //multiply by a scalar constant
	Matrix<Tp> temp(rows(), cols()); //intialize the temporary matrix

	temp.data = data * y;
	return temp; //return the matrix
}

template<class Tp>
Matrix<Tp>&	Matrix<Tp>::operator^(const Matrix<Tp> y) // Jiadi
{
	data = data.mul(y.data);
	return *this;
}

template<class Tp>
Matrix<Tp> Matrix<Tp>::operator / (const Tp y) // Jiadi
{  //multiply by a scalar constant
	Matrix<Tp> temp(rows(), cols()); //intialize the temporary matrix

	temp.data = data / y;
	return temp; //return the matrix
}


template<class Tp>
__forceinline Tp& Matrix<Tp>::operator ()(const int row, const int col) // Jiadi
{
	return data.at<Tp>(row, col);
}

template<class Tp>
__forceinline Tp& Matrix<Tp>::operator ()(const long index) // Jiadi
{
	return data.at<Tp>(index / ncol, index % ncol);
}

template<class Tp>
Matrix<Tp> & Matrix<Tp>::operator=(const Matrix<Tp>& x) // Jiadi
{//performs assignment statement

	//beware of self assignments : x = x
	if (this != &x)
	{
		nrow = x.nrow; ncol = x.ncol;
		data = x.data.clone();
	}
	return *this;
}

template<class Tp>
Tp Matrix<Tp>::Dist(const Matrix<Tp> &a)
{
	Matrix<Tp>	temp;

	if (a.cols() != 1)		return 0;
	if (cols() != 1)			return 0;
	if (rows() != a.rows())	return 0;

	temp = *this - a;
	temp = temp.T()*temp;

	return temp.Data(0);
}

template<class Tp1, class Tp2>
void CopyMatrix(Matrix<Tp1>& mx_dest, const Matrix<Tp2>& mx_src) // Jiadi
{
	mx_dest.SetDimension(mx_src.rows(), mx_src.cols());

	for (int i = 0; i < mx_src.rows(); i++)
		for (int j = 0; j < mx_src.cols(); j++)
			mx_dest(i, j) = (Tp1)(mx_src.Data(i, j));
}

template<class Tp1, class Tp2>
void CopyMatrix(Vector<Tp1>& mx_dest, const Matrix<Tp2>& mx_src) // Jiadi
{
	mx_dest.SetDimension(mx_src.rows(), mx_src.cols());

	for (int i = 0; i < mx_src.rows(); i++)
	for (int j = 0; j < mx_src.cols(); j++)
		mx_dest(i, j) = (Tp1)(mx_src.Data(i, j));
}



template<class Tp>
Matrix<Tp> Matrix<Tp>::T() const // Jiadi
{//transposes of a matrix

	Matrix temp(ncol, nrow); //intialize a temp matrix
	temp.data = data.t();
	return temp;
}


// set this matrix to be I matrix
template<class Tp>   // Jiadi! memory leaks here
void Matrix<Tp>::Identity(void)
{
	data = cv::Mat::eye(nrow, ncol, cv::DataType<Tp>::type);
}


// set this matrix to be zero matrix
template<class Tp> 
void Matrix<Tp>::Zero(void)  // Jiadi! memory leaks here
{
	data = cv::Mat::zeros(nrow, ncol, cv::DataType<Tp>::type);
}

template<class Tp>
Tp Matrix<Tp>::Data(const int row, const int col) const // Jiadi
{
	return data.at<Tp>(row, col);
}

template<class Tp>
Tp Matrix<Tp>::Data(const int index) const // Jiadi
{
	return data.at<Tp>(index / ncol, index % ncol);
}

template<class Tp>
void Matrix<Tp>::GetPos(const long index, long &j, long &i) // Jiadi
{
	j = index / ncol;
	i = index % ncol;
}

template<class Tp>
__forceinline	void Matrix<Tp>::InitValue(Tp value) // Jiadi
{
	data.setTo(cv::Scalar::all(value));
}

// horizontal convolution
template<class Tp>
Matrix<Tp> Matrix<Tp>::ConvHor(const Matrix<Tp>& kern) // Jiadi
{
	Matrix<Tp>	I(nrow, ncol);
	cv::filter2D(data, I.data, -1, kern.data);
	I = I*-1;
	return I;
}

// vertical convolution
template<class Tp>
Matrix<Tp> Matrix<Tp>::ConvVer(const Matrix<Tp>& kern) // Jiadi
{
	Matrix<Tp>	I(nrow, ncol);
	cv::filter2D(data, I.data, -1, kern.data);
	I = I*-1;
	return I;
}


template<class Tp>
void Matrix<Tp>::SetGaussian(int dim, double sigma, int direvitive)
{
	Matrix<Tp>	op;
	int			size, i, j, u;
	double		x, y;

	size = ((int)sigma) * 6 + 1;
	u = (size - 1) / 2;

	assert(size > 0);

	// gaussian kernel
	if (dim == 1)
	{
		op.SetDimension(1, size);
		op.Zero();
		for (i = 0; i<size; i++)
		{
			x = double(i - u);
			switch (direvitive)
			{
			case 0:	// Gaussian function
				op(0, i) = (Tp)gauss(x, sigma);
				break;
			case 1:	// First dirivtive of Gaussian
				op(0, i) = (Tp)dgauss(x, sigma);
				break;
			case 2: // Second dirivtive of Gaussian
				op(0, i) = (Tp)d2gauss(x, sigma);
				break;
			}
		}
	}
	else
	{
		double	std2 = sigma*sigma;
		double	h1;

		op.SetDimension(size, size);
		op.Zero();

		switch (direvitive)
		{
		case 0:
			for (j = 0; j<size; j++)
			for (i = 0; i<size; i++)
			{
				x = double(j - u);
				y = double(i - u);

				op(j, i) = (Tp)gauss(sqrt(x*x + y*y), sigma);
			}
			op = op / op.Sum();
			break;
		case 2:
			for (j = 0; j<size; j++)
			for (i = 0; i<size; i++)
			{
				x = double(j - u);
				y = double(i - u);

				h1 = exp(-(x*x + y*y) / (2 * std2));
				op(j, i) = (Tp)(h1*(x*x + y*y - 2 * std2) / (2 * LPI*(pow(sigma, 6))));
			}
			break;
		}
	}
	*this = op;
}


__forceinline double	gauss(double x, double sigma)
{
	return exp(-x*x / (2 * sigma*sigma)) / (sigma*sqrt(2 * LPI));
};
__forceinline double	dgauss(double x, double sigma)
{
	return -x*gauss(x, sigma) / (sigma*sigma);
};
__forceinline double	d2gauss(double x, double sigma)
{
	return (x*x / pow(sigma, 4) - 1 / pow(sigma, 2))*gauss(x, sigma);
};

// Gabor Filter
// make gabor filter                                                   2
//               k*k                k*k       2                  sigma
//  Pk(x) = --------------exp(- -------------x )(exp(ikx)-exp(- --------)
//          (sigma*sigma)       (sigma*sigma)                      2
//
template<class Tp>
void Matrix<Tp>::SetGabor(const double k, const double sigma, 
	const double theta /* in degree */)								// Jiadi
{
	int half_size, w, h, i, j;

	// size of the filter
	if (k>1e-10)
		half_size = (int)floor(sigma / k*1.5);
	else
		half_size = (int)floor(sigma*1.5);
	w = 2 * half_size + 1;
	h = 2 * half_size + 1;

	// initilization
	// gaussian kernel
	Matrix<Tp>	F(1, w), G;
	double		x, sigma_s, k_s;

	k_s = k*k;
	sigma_s = sigma*sigma;
	for (i = 0; i<w; i++)
	{
		x = (double)(i - half_size);
		x = x*x;
		F(i) = (Tp)(k_s / sigma_s*exp(-k_s / (2 * sigma_s)*x));
	}																		  
	G = F.T()*F;

	// sinusoid kernel
	// to grad
	Matrix<Tp>		P(h, w);
	Matrix<double>	r(2, 2), temp(2, 1), u;
	double			angle;
	angle = theta / 180 * LPI;
	r(0, 0) = cos(angle);
	r(0, 1) = -sin(angle);
	r(1, 0) = sin(angle);
	r(1, 1) = cos(angle);

	for (j = 0; j<h; j++)
		for (i = 0; i<w; i++)
		{
			temp(0) = j - half_size;
			temp(1) = i - half_size;
			u = r*temp;
			P(j, i) = (Tp)(cos(u(0)*k));
		}

	// Gabor filter
	(*this) = G^P;
	(*this) = (*this) / Sum();
}

template<class Tp>
void Matrix<Tp>::Eigen(Matrix<Tp>& eigenvalue, Matrix<Tp>& eigenvector) // Jiadi
{
	if (nrow != ncol)
		return;
	int n = nrow;
	eigenvalue.SetDimension(n, 1);
	eigenvector.SetDimension(n, n);
	
	cv::eigen(data, eigenvalue.data, eigenvector.data);
	eigenvector.data = eigenvector.data.t();
}

template<class Tp>
Tp Matrix<Tp>::GetMin(void) const // Jiadi 
{
	double value; 

	if (size()>0)
	{
		cv::minMaxIdx(data, &value, 0);
	}

	return (Tp)value;
}

template<class Tp>
Tp Matrix<Tp>::GetMax(void) const // Jiadi 
{
	double value;

	if (size()>0)
	{
		cv::minMaxIdx(data, 0, &value);
	}

	return (Tp)value;
}

template<class Tp>
void Matrix<Tp>::GetMinMax(Tp &value_min, Tp &value_max) // Jiadi
{
	double vmin, vmax;
	if (size()>0)
	{
		cv::minMaxIdx(data, &vmin, &vmax);
		value_min = (Tp)vmin;
		value_max = (Tp)vmax;
	}
}


// the summation of all the elements
template<class Tp>
Tp Matrix<Tp>::Sum(void) const // Jiadi
{
	return Tp(sum(data).val[0]);
}

// the summation of the square of every element
template<class Tp>
Tp Matrix<Tp>::SumL2(void) const  // Jiadi
{
	return Tp(pow(norm(data), 2));
}

// normalize the matrix for its sumation to zero
template<class Tp>
void Matrix<Tp>::NormToZero(void) // Jiadi
{
	Tp		sum = Sum(), g;

	g = (Tp)((sum) / ((double)size()));
	if (sum != 0)
		data = data - g;
}

template<class Tp>
Tp Matrix<Tp>::Mean(void) const // Jiadi
{
	Tp	value;
	if (size()>0)
		value = (Tp)(mean(data)[0]);
	return value;
}

template<class Tp>
Matrix<Tp> Matrix<Tp>::mabs(void) const // Jiadi
{
	Matrix<Tp> I;

	I.SetDimension(nrow, ncol);
	I.data = abs(data);
	return I;
}



template<class Tp>
double Matrix<Tp>::Det() const  // Jiadi
{//returns the value of the determinanat
	double d = cv::determinant(data); //interchange flag
	return fabs(d);
}

template<class Tp>
double Matrix<Tp>::DetNoZero() const  // Jiadi
{
	double det = Det();
	if (det<1.0)
		return 1.0;
	else
		return det;
}

template<class Tp>
Matrix<Tp> Matrix<Tp>::Inv() const // Jiadi
{//returns the inverse of the matrix
	Matrix<Tp> result(rows(), cols());    //the final result

	result.data = data.inv();
	return result;
}

template<class Tp>
class SimpleMatrix3D
{
public:
	// constructions & destructions
	SimpleMatrix3D();
	SimpleMatrix3D(int dim1, int dim2, int dim3);
	~SimpleMatrix3D();

	// operators
	Tp&				operator()(const int dim1, const int dim2, const int dim3);
	SimpleMatrix3D<Tp>&		operator=(const SimpleMatrix3D<Tp>& x);

	// operations
	int				SetDimension(int dim1, int dim2, int dim3);
	__forceinline long		Dim1() const{ return ndim1; };
	__forceinline long		Dim2() const{ return ndim2; };
	__forceinline long		Dim3() const{ return ndim3; };
	__forceinline long		size(void) const { return ((long)ndim1)*ndim2*ndim3; };
	__forceinline bool		Valid(const int dim1, const int dim2, const int dim3);
	__forceinline Tp		Data(const int dim1, const int dim2, const int dim3) const;
	__forceinline long		GetIndex(const int dim1, const int dim2, const int dim3) const;
	__forceinline void		GetIndex(int &dim1, int &dim2, int &dim3, const long index) const;
	__forceinline	void	InitValue(Tp x);

protected:
	SimpleMatrix<Tp>	*p_mx;
	int					ndim1, ndim2, ndim3;
};


template<class Tp>
SimpleMatrix3D<Tp>::SimpleMatrix3D()
{
	ndim1 = 0;
	ndim2 = 0;
	ndim3 = 0;
	p_mx = NULL;
}

template<class Tp>
SimpleMatrix3D<Tp>::~SimpleMatrix3D()
{
	ndim1 = 0;
	ndim2 = 0;
	ndim3 = 0;
	if (p_mx != NULL)
		delete[]p_mx;
}

// operators
template<class Tp>
Tp&	SimpleMatrix3D<Tp>::operator()(const int dim1, const int dim2, const int dim3)
{
	assert(dim1 >= 0 && dim1<ndim1);
	return p_mx[dim1](dim2, dim3);
}

template<class Tp>
SimpleMatrix3D<Tp> & SimpleMatrix3D<Tp>::operator=(const SimpleMatrix3D<Tp>& x)
{
	if (this != &x)
	{
		SetDimension(x.Dim1(), x.Dim2(), x.Dim3());
		for (int i = 0; i<ndim1; i++)
			p_mx[i] = x.p_mx[i];
	}

	return *this;
}

template<class Tp>
int SimpleMatrix3D<Tp>::SetDimension(int dim1, int dim2, int dim3)
{
	if (ndim1 != dim1)
	{
		if (p_mx != NULL)
			delete[]p_mx;
		p_mx = new SimpleMatrix<Tp>[dim1];
		ndim1 = dim1;
	}
	if (ndim2 != dim2 || ndim3 != dim3)
	{
		ndim2 = dim2;
		ndim3 = dim3;

		for (int i = 0; i<ndim1; i++)
			p_mx[i].SetDimension(ndim2, ndim3);
	}
	return size();
}

// operators
template<class Tp>
Tp	SimpleMatrix3D<Tp>::Data(const int dim1, const int dim2, const int dim3) const
{
	assert(dim1 >= 0 && dim1<ndim1);
	return p_mx[dim1].Data(dim2, dim3);
}


template<class Tp>
__forceinline long	SimpleMatrix3D<Tp>::GetIndex(const int dim1, const int dim2, const int dim3) const
{
	return Dim2()*Dim3()*dim1 + Dim3()*dim2 + dim3;
}

template<class Tp>
__forceinline void	SimpleMatrix3D<Tp>::GetIndex(int &dim1, int &dim2, int &dim3, const long index) const
{
	long temp = index;

	dim1 = temp / (Dim2()*Dim3());
	temp = temp - dim1*Dim2()*Dim3();
	dim2 = temp / Dim3();
	dim3 = temp - dim2*Dim3();
}

template<class Tp>
__forceinline void SimpleMatrix3D<Tp>::InitValue(Tp x)
{
	int i, j, k;

	for (i = 0; i<Dim1(); i++)
	for (j = 0; j<Dim2(); j++)
	for (k = 0; k<Dim3(); k++)
		(*this)(i, j, k) = x;
}

template<class Tp>
__forceinline bool	SimpleMatrix3D<Tp>::Valid(const int dim1, const int dim2, const int dim3)
{
	if (dim1 >= 0 && dim1<Dim1() && dim2 >= 0 && dim2<Dim2() && dim3 >= 0 && dim3<Dim3())
		return true;
	else
		return false;
}

#endif


